using Unity.Burst.Intrinsics;
using Unity.Collections;
using Unity.Entities;
using Unity.Transforms;
using UnityEngine;

namespace PathFinding
{
    public partial struct PathSteeringSystem : ISystem
    {

        public void Update(ref SystemState state)
        {
            var pathQuery = state.EntityManager.GetComponentData<PathQuerySystem.Singleton>(state.SystemHandle);
            new PathSteeringJob()
            {
                PathQuery = pathQuery,
            }.ScheduleParallel();
            pathQuery.World.AddDependency(state.Dependency);
        }
        partial struct PathSteeringJob : IJobEntity, IJobEntityChunkBeginEnd
        {
            public PathQuerySystem.Singleton PathQuery;
            
            AStarFunnel Funnel;
            public void Execute([EntityIndexInChunk] int index, ref DynamicBuffer<PathNode> nodes, ref AgentPath path, ref AgentBody body, in LocalTransform transform)
            {
                if (!PathQuery.IsValid(path.Location.polygon) ||
                    PathMath.SqrDistance(transform.Position, body.Destination) > 0.01f)
                {
                    var location = PathQuery.MapLocation(transform.Position, path.MappingExtent, path.AgentTypeId, path.AreaMask);

                    if (location.polygon.IsNull())
                    {
                        UnityEngine.Debug.LogWarning("Failed to map agent position to nav mesh location. This can happen either if nav mesh is not present or property MappingExtent value is too low.");
                        return;
                    }

                    // In case location polygon changed, we need to progress path so funnel would work correctly
                    if (location.polygon != path.Location.polygon)
                    {
                        PathQuery.ProgressPath(ref nodes, path.Location.polygon, location.polygon);
                    }
                    path.Location = location;
                }

                if (body.IsStopped)
                    return;

                if (path.State != AgentPathState.FinishedFullPath && path.State != AgentPathState.Failed)
                {
                    // TODO: Check would be best way to handle un finished path
                    // Now it continues to move to destination without accounting navmesh
                    //body.Force = 0;
                    //destination.RemainingDistance = 0;
                    return;
                }
                if(!PathQuery.IsValid(path.EndLocation.polygon) || PathMath.SqrDistance(body.Destination, path.EndLocation.position) > 0.01f)
                {
                    var endLocation = PathQuery.MapLocation(body.Destination, path.MappingExtent, path.AgentTypeId, path.AreaMask);
                    // Handle case if failde to map location
                    if (endLocation.polygon.IsNull())
                    {
                        UnityEngine.Debug.LogWarning("Failed to map agent destination to nav mesh location. This can happen either if nav mesh is not present or property MappingExtent value is too low.");
                    }

                    body.Destination = endLocation.position;
                    
                    if (endLocation.polygon != path.EndLocation.polygon)
                    {
                        path.State = AgentPathState.WaitingNewPath;
                        return;
                    }

                    path.EndLocation = endLocation;
                }
                if (path.State == AgentPathState.Failed)
                {
                    body.Stop();
                    return;
                }
                var polygons = nodes.AsNativeArray().Reinterpret<PathPolygonId>();
                if (polygons.Length == 0)
                {
                    body.Stop();
                    return;
                }
                //根据nodes不断的往前推进
                if(PathQuery.TryCreateFunnel(ref Funnel, polygons, transform.Position, body.Destination))
                {
                    var locations = Funnel.AsLocations();
                    if (locations.Length > 1)
                    {
                        body.Force = Vector3.Normalize(locations[1].position - transform.Position);
                        body.RemainingDistance = Funnel.IsEndReachable ? Funnel.GetCornersDistance() : float.MaxValue;
                        
                        //处理离散点
                    }
                }
                else
                {
                    body.Stop();
                }
                
            }
            public bool OnChunkBegin(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask)
            {
                // Create tunnel that will allow finding optimal path is navmesh nodes
                // Only 4 positions constructed as, because of local changes there is change path will need change
                Funnel = new AStarFunnel(4, Allocator.Temp);
                return true;
            }

            public void OnChunkEnd(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask,
                bool chunkWasExecuted)
            {
                //throw new NotImplementedException();
            }
        }
    }
}